#include <soc/rtc_cntl_reg.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <BluetoothSerial.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLEScan.h>
#include <BLE2902.h>
#include <math.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#define Service_UUID "0000ffe0-0000-1000-8000-00805f9b34fb"
#define CharacRDWT_UUID "0000ffe1-0000-1000-8000-00805f9b34fb"
#define CharacWRITE_UUID "0000ffe3-0000-1000-8000-00805f9b34fb"
#define CharacNOTI_UUID "0000ffe4-0000-1000-8000-00805f9b34fb"
#define CharacREAD_UUID "0000ffe5-0000-1000-8000-00805f9b34fb"
#define CharacINDI_UUID "0000ffe7-0000-1000-8000-00805f9b34fb"

HardwareSerial UART0(0);//定义串口对象

const char myBleName[] = "BPLUG-ESP-TX";//低功耗蓝牙设备名称
uint8_t BleMacAddr[6] = {0x84,0xC2,0xE4,0x03,0x02,(uint8_t)(0x06-2)};//低功耗蓝牙MAC地址
//这个地址被设置时会自动+2,故这里提前-2(也有说是别的处理,可用调试软件查看确定)

BLEServer *pServer = NULL;
BLEService *pService = NULL;

BLECharacteristic *pCharacRDWT = NULL;
BLECharacteristic *pCharacWRITE = NULL;

BLECharacteristic *pCharacINDI = NULL;
BLECharacteristic *pCharacNOTI = NULL;
BLECharacteristic *pCharacREAD = NULL;
BLEAdvertising *pAdvertising = NULL;

uint8_t XboxData[20] = {0,20, 0,0, 0,0, 0,0,0,0,0,0,0,0, 0,0,0,0,0,0};//Xbox数据包

int16_t RkValue[4] = {0,0,0,0};//摇杆值

void setup(){
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);//关闭低电压检测,避免无限重启
  UART0.begin(115200,SERIAL_8N1,3,1,false,20000UL);
  UART0.printf("\nStart\n");

  pinMode(0,INPUT);//板载按键
  //pinMode(2,OUTPUT);//板载LED

  esp_base_mac_addr_set(BleMacAddr);//设置蓝牙MAC地址
  BLEDevice::init(myBleName);//设置设备名称并初始化
  
  pServer = BLEDevice::createServer();//创建BLE服务器
  //pServer->setCallbacks(new MyServerCallbacks());//设置连接和断开调用类
  pService = pServer->createService(Service_UUID);//创建BLE服务

  pCharacRDWT = pService->createCharacteristic(CharacRDWT_UUID,BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
  pCharacWRITE = pService->createCharacteristic(CharacWRITE_UUID,BLECharacteristic::PROPERTY_WRITE);
  pCharacNOTI = pService->createCharacteristic(CharacNOTI_UUID,BLECharacteristic::PROPERTY_NOTIFY);
  pCharacNOTI->addDescriptor(new BLE2902());//启动通知
  pCharacREAD = pService->createCharacteristic(CharacNOTI_UUID,BLECharacteristic::PROPERTY_READ);
  pCharacINDI = pService->createCharacteristic(CharacINDI_UUID,BLECharacteristic::PROPERTY_INDICATE);
  
  pService->start();
  
  pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(Service_UUID);
  pAdvertising->start();//开始广播

  //xTaskCreatePinnedToCore(Task1code, "Task1", 10000, NULL, 0, NULL,  1);
}
//void Task1code(void *pvParameters){
void loop(){
  disableCore1WDT();//失能1号核心看门狗
  TickType_t xLastWakeTime;
  vTaskDelay(10);
  
  while(1){
    xLastWakeTime = xTaskGetTickCount();
    static uint8_t printTick = 0;

    if(pServer->getConnectedCount() == 0){}//无客户端连接
    else//有客户端连接
    {
      //部分按键演示:
      if(printTick == 0){//每秒切换一个
        if(XboxData[2]) XboxData[2] <<= 1;//流水按键
        else XboxData[2] = 1;
      }
      XboxData[3] = !digitalRead(0);//板载按键
      //digitalWrite(2, !digitalRead(0));//控制板载LED
      //扳机演示-模拟渐变:
      uint16_t trigTmp = (millis() / 8) % 512;
      XboxData[4] = trigTmp < 256 ? trigTmp : 511 - trigTmp;
      XboxData[5] = trigTmp > 255 ? trigTmp : 511 - trigTmp;
      //摇杆演示-模拟画圆:
      RkValue[0] = 32767 * sin((float)(millis() % 6283) / 1000.0);//相位0度
      RkValue[1] = 32767 * sin((float)(millis() % 6283) / 1000.0 + 1.5708);//相位90度
      RkValue[2] = 32767 * sin((float)(millis() % 6283) / 1000.0 + 3.1416);//相位180度
      RkValue[3] = 32767 * sin((float)(millis() % 6283) / 1000.0 + 4.7124);//相位270度
      memcpy(XboxData + 6, RkValue, 8);//摇杆值填入数据包
      
      if(pCharacNOTI){//数据通过这个特征发送
        pCharacNOTI->setValue(XboxData, 14);//设置特征值
        pCharacNOTI->notify();//通知客户端
      }
//      if(pCharacREAD){
//        pCharacREAD->setValue(XboxData, sizeof(XboxData));
//      }
//      if(pCharacRDWT){
//        std::string characValue = pCharacRDWT->getValue();//获取特征值
//      }
//      if(pCharacINDI){
//        //std::string characValue = pCharacINDI->getValue();//获取特征值
//      }
//      if(pCharacWRITE){
//        std::string characValue = pCharacWRITE->getValue();//获取特征值
//      }//注释掉的特征暂不使用
    }
  
    if(printTick == 0){
      //UART0.printf("CNT:%d\n",pServer->getConnectedCount());//打印连接数
    }
    printTick = (printTick + 1) % 50;//计数分频变量
    
    //delay(10);
    vTaskDelayUntil(&xLastWakeTime,20);//20ms周期
  }
}







//END
